#include "F4_flash.h"

#define FLASH_SAVE_ADDR  0x08040000 	
u16 Flash_Parameter[10];

float Position1=SERVO_INIT,Position2=SERVO_INIT,Position3=SERVO_INIT,Position4=SERVO_INIT; 
float Velocity1=0,Velocity2=0,Velocity3=0,Velocity4=0;     
float	Position_KP=2,Position_KI=0,Position_KD=1;  
short Moveit_Angle1_init=0,Moveit_Angle2_init=0,Moveit_Angle3_init=0,Moveit_Angle4_init=0; 
int Servo_init_angle_adjust=0;




u16 STMFLASH_ReadHalfWord(u32 faddr)
{
	return *(vu16*)faddr; 
}  



uint16_t STMFLASH_GetFlashSector(u32 addr)
{
	if(addr<ADDR_FLASH_SECTOR_1)return FLASH_Sector_0;
	else if(addr<ADDR_FLASH_SECTOR_2)return FLASH_Sector_1;
	else if(addr<ADDR_FLASH_SECTOR_3)return FLASH_Sector_2;
	else if(addr<ADDR_FLASH_SECTOR_4)return FLASH_Sector_3;
	else if(addr<ADDR_FLASH_SECTOR_5)return FLASH_Sector_4;
	else if(addr<ADDR_FLASH_SECTOR_6)return FLASH_Sector_5;
	else if(addr<ADDR_FLASH_SECTOR_7)return FLASH_Sector_6;
	else if(addr<ADDR_FLASH_SECTOR_8)return FLASH_Sector_7;
	else if(addr<ADDR_FLASH_SECTOR_9)return FLASH_Sector_8;
	else if(addr<ADDR_FLASH_SECTOR_10)return FLASH_Sector_9;
	else if(addr<ADDR_FLASH_SECTOR_11)return FLASH_Sector_10; 
	return FLASH_Sector_11;	
}










void STMFLASH_Write(u32 WriteAddr,u16 *pBuffer,u32 NumToWrite)	
{ 
  FLASH_Status status = FLASH_COMPLETE;
	u32 addrx=0;
	u32 endaddr=0;	
  if(WriteAddr<STM32_FLASH_BASE||WriteAddr%2)return;	
	FLASH_Unlock();									
  FLASH_DataCacheCmd(DISABLE);		
 		
	addrx=WriteAddr;				
	endaddr=WriteAddr+NumToWrite*2;	
	if(addrx<0X1FFF0000)			
	{
		while(addrx<endaddr)		
		{
			if(STMFLASH_ReadHalfWord(addrx)!=0XFFFF)
			{   
				status=FLASH_EraseSector(STMFLASH_GetFlashSector(addrx),VoltageRange_3);
				if(status!=FLASH_COMPLETE)break;	
			}else addrx+=2;
		} 
	}
	if(status==FLASH_COMPLETE)
	{
		while(WriteAddr<endaddr)
		{
			if(FLASH_ProgramHalfWord(WriteAddr,*pBuffer)!=FLASH_COMPLETE)
			{ 
				break;	
			}
			WriteAddr+=2;
			pBuffer++;
		} 
	}
  FLASH_DataCacheCmd(ENABLE);	
	FLASH_Lock();
} 





void STMFLASH_Read(u32 ReadAddr,u16 *pBuffer,u32 NumToRead)   	
{
	u32 i;
	for(i=0;i<NumToRead;i++)
	{
		pBuffer[i]=STMFLASH_ReadHalfWord(ReadAddr);
		ReadAddr+=2;
	}
}


void Flash_Read(void)
{
	STMFLASH_Read(FLASH_SAVE_ADDR,(u16*)Flash_Parameter,10);
	if(Flash_Parameter[0]==65535&&Flash_Parameter[1]==65535&&Flash_Parameter[2]==65535&&Flash_Parameter[3]==65535)
	{
    Moveit_Angle1_init=0,Moveit_Angle2_init=0,Moveit_Angle3_init=0,Moveit_Angle4_init=0;
	}
  else 
	{		
		Moveit_Angle1_init=Flash_Parameter[0];	
		Moveit_Angle2_init=Flash_Parameter[1];	
		Moveit_Angle3_init=Flash_Parameter[2];	
		Moveit_Angle4_init=Flash_Parameter[3];	
	
	}
}	

void Flash_Write(void)
{
	Flash_Parameter[0]=(u16)Moveit_Angle1_init;	
	Flash_Parameter[1]=(u16)Moveit_Angle2_init;	
	Flash_Parameter[2]=(u16)Moveit_Angle3_init;	
	Flash_Parameter[3]=(u16)Moveit_Angle4_init;	
	STMFLASH_Write(FLASH_SAVE_ADDR,(u16*)Flash_Parameter,10);	
}	























